package org.hxzon.cyclone.contract;

import java.util.List;

import org.hxzon.cyclone.Vector3;

public class ContactResolver {
    //附加，限制速度的处理次数
    protected long velocityIterations;

    //附加，限制位置的处理次数 
    protected long positionIterations;

    //附加，记录速度的已处理的次数
    public long velocityIterationsUsed;

    //附加，记录位置的已处理次数 
    public long positionIterationsUsed;

    /**
     * To avoid instability velocities smaller
     * than this value are considered to be zero. Too small and the
     * simulation may be unstable, too large and the bodies may
     * interpenetrate visually. A good starting point is the default
     * of 0.01.
     */
    protected double velocityEpsilon;

    /**
     * To avoid instability penetrations
     * smaller than this value are considered to be not interpenetrating.
     * Too small and the simulation may be unstable, too large and the
     * bodies may interpenetrate visually. A good starting point is
     * the default of0.01.
     */
    protected double positionEpsilon;

    /**
     * Keeps track of whether the internal settings are valid.
     */
    private boolean validSettings;

    /**
     * Creates a new contact resolver with the given number of iterations
     * per resolution call, and optional epsilon values.
     */
    //unsigned,real,real
    public ContactResolver(long iterations, double velocityEpsilon,//=(real)0.01,
            double positionEpsilon) {//=(real)0.01
        setIterations(iterations, iterations);
        setEpsilon(velocityEpsilon, positionEpsilon);
    }

    public ContactResolver(long iterations) {
        this(iterations, 0.01, 0.01);
    }

    /**
     * Creates a new contact resolver with the given number of iterations
     * for each kind of resolution, and optional epsilon values.
     */
    //unsigned,unsigned,real,real
    public ContactResolver(long velocityIterations, long positionIterations, //
            double velocityEpsilon,//=(real)0.01,
            double positionEpsilon) {//=(real)0.01
        setIterations(velocityIterations);
        setEpsilon(velocityEpsilon, positionEpsilon);
    }

    /**
     * Returns true if the resolver has valid settings and is ready to go.
     */
    public boolean isValid() {
        return (velocityIterations > 0) && (positionIterations > 0) //
                && (positionEpsilon >= 0.0f) && (positionEpsilon >= 0.0f);
    }

    public void setIterations(long velocityIterations, long positionIterations) {
        this.velocityIterations = velocityIterations;
        this.positionIterations = positionIterations;
    }

    public void setIterations(long iterations) {
        setIterations(iterations, iterations);
    }

    /**
     * Sets the tolerance value for both velocity and position.
     */
    public void setEpsilon(double velocityEpsilon, double positionEpsilon) {
        this.velocityEpsilon = velocityEpsilon;
        this.positionEpsilon = positionEpsilon;
    }

    /**
     * Resolves a set of contacts for both penetration and velocity.
     *
     * Contacts that cannot interact with
     * each other should be passed to separate calls to resolveContacts,
     * as the resolution algorithm takes much longer for lots of
     * contacts than it does for the same number of contacts in small
     * sets.
     *
     * @param contactArray Pointer to an array of contact objects.
     *
     * @param numContacts The number of contacts in the array to resolve.
     *
     * @param numIterations The number of iterations through the
     * resolution algorithm. This should be at least the number of
     * contacts (otherwise some constraints will not be resolved -
     * although sometimes this is not noticable). If the iterations are
     * not needed they will not be used, so adding more iterations may
     * not make any difference. In some cases you would need millions
     * of iterations. Think about the number of iterations as a bound:
     * if you specify a large number, sometimes the algorithm WILL use
     * it, and you may drop lots of frames.
     *
     * @param duration The duration of the previous integration step.
     * This is used to compensate for forces applied.
     */
    public void resolveContacts(List<Contact> contacts, double duration) {
        // Make sure we have something to do.
        if (contacts.isEmpty()) {
            return;
        }
        if (!isValid()) {
            return;
        }

        // Prepare the contacts for processing
        prepareContacts(contacts, duration);

        // Resolve the interpenetration problems with the contacts.
        adjustPositions(contacts, duration);

        // Resolve the velocity problems with the contacts.
        adjustVelocities(contacts, duration);
    }

    /**
     * Sets up contacts ready for processing. This makes sure their
     * internal data is configured correctly and the correct set of bodies
     * is made alive.
     */
    protected void prepareContacts(List<Contact> contacts, double duration) {
        // Generate contact velocity and axis information.
        for (Contact contact : contacts) {
            // Calculate the internal contact data (inertia, basis, etc).
            contact.calculateInternals(duration);
        }
    }

    /**
     * Resolves the velocity issues with the given array of constraints,
     * using the given number of iterations.
     */
    protected void adjustVelocities(List<Contact> c, double duration) {
        Vector3[] velocityChange = new Vector3[2];
        Vector3[] rotationChange = new Vector3[2];
        Vector3 deltaVel = new Vector3();

        // iteratively handle impacts in order of severity.
        velocityIterationsUsed = 0;
        while (velocityIterationsUsed < velocityIterations) {
            // Find contact with maximum magnitude of probable velocity change.
            double max = velocityEpsilon;
            int index = c.size();
            for (int i = 0; i < c.size(); i++) {
                if (c.get(i).desiredDeltaVelocity > max) {
                    max = c.get(i).desiredDeltaVelocity;
                    index = i;
                }
            }
            if (index == c.size()) {
                break;
            }

            // Match the awake state at the contact
            c.get(index).matchAwakeState();

            // Do the resolution on the contact that came out top.
            c.get(index).applyVelocityChange(velocityChange, rotationChange);

            // With the change in velocity of the two bodies, the update of
            // contact velocities means that some of the relative closing
            // velocities need recomputing.
            for (int i = 0; i < c.size(); i++) {
                // Check each body in the contact
                for (int b = 0; b < 2; b++) {
                    if (c.get(i).body[b] != null) {
                        // Check for a match with each body in the newly
                        // resolved contact
                        for (int d = 0; d < 2; d++) {
                            if (c.get(i).body[b] == c.get(index).body[d]) {
                                deltaVel = velocityChange[d].add(rotationChange[d].vectorProduct(c.get(i).relativeContactPosition[b]));

                                // The sign of the change is negative if we're dealing
                                // with the second body in a contact.
                                c.get(i).contactVelocity.addUpdate(c.get(i).contactToWorld.transformTranspose(deltaVel).multi((b != 0 ? -1 : 1)));//(b?-1:1)
                                c.get(i).calculateDesiredDeltaVelocity(duration);
                            }
                        }
                    }
                }
            }
            velocityIterationsUsed++;
        }
    }

    /**
     * Resolves the positional issues with the given array of constraints,
     * using the given number of iterations.
     */
    protected void adjustPositions(List<Contact> c, double duration) {
        int i, index;
        Vector3[] linearChange = new Vector3[2];
        Vector3[] angularChange = new Vector3[2];
        double max;
        Vector3 deltaPosition = new Vector3();

        // iteratively resolve interpenetrations in order of severity.
        positionIterationsUsed = 0;
        while (positionIterationsUsed < positionIterations) {
            // Find biggest penetration
            max = positionEpsilon;
            index = c.size();
            for (i = 0; i < c.size(); i++) {
                if (c.get(i).penetration > max) {
                    max = c.get(i).penetration;
                    index = i;
                }
            }
            if (index == c.size()) {
                break;
            }
            // Match the awake state at the contact
            c.get(index).matchAwakeState();

            // Resolve the penetration.
            c.get(index).applyPositionChange(linearChange, angularChange, max);

            // Again this action may have changed the penetration of other
            // bodies, so we update contacts.
            for (i = 0; i < c.size(); i++) {
                // Check each body in the contact
                for (int b = 0; b < 2; b++) {
                    if (c.get(i).body[b] != null) {
                        // Check for a match with each body in the newly
                        // resolved contact
                        for (int d = 0; d < 2; d++) {
                            if (c.get(i).body[b] == c.get(index).body[d]) {
                                deltaPosition = linearChange[d].add(angularChange[d].vectorProduct(c.get(i).relativeContactPosition[b]));

                                // The sign of the change is positive if we're
                                // dealing with the second body in a contact
                                // and negative otherwise (because we're
                                // subtracting the resolution)..
                                c.get(i).penetration += deltaPosition.scalarProduct(c.get(i).contactNormal) * (b != 0 ? 1 : -1);//(b?1:-1)
                            }
                        }
                    }
                }
            }
            positionIterationsUsed++;
        }
    }
}